#include "findway.hpp"
#include "game.hpp"
#include <queue>
#include <iostream>
#include <vector>
#include <cmath>

extern cGame * game;

std::vector <sPoint> cFindWay::findWay(cMap * map, sPoint start, sPoint end)
{
    //jesli nie ma przeszkod pomiedzy punktami start i end to zwraca nam tylko ten 1 punkt
    if (!obstacleBetweenPoints(start, end, map))
    {
        if (!checkCollision(map, &end))
        {
            std::vector <sPoint> ret;
            ret.push_back(end);
            return ret;
        }
    }

    sPoint s = start/TILE_SIZE;
    sPoint e = end/TILE_SIZE;

    prepareTile(map->size);

    sPoint w(0, 0);
    sPoint v(0, 0);
    std::queue <sPoint> Q;

    Q.push(s);
    tile[(int)s.x][(int)s.y] = 0;

    while (!Q.empty())
    {
        v = Q.front();
        Q.pop();

        for (int i = 0; i < 8; i++)
        {
            w = v + MOOR[i];

            if (!map->isOnMap(w*TILE_SIZE)) continue;
            if (map->fogOn && !map->fogOfWar[(int)w.x][(int)w.y]) continue;
            if (map->map_green[(int)w.x][(int)w.y] != -1) continue;
            if (tile[(int)w.x][(int)w.y] != UNVISITED_FIELD) continue;
            if (map->tile[map->map[(int)w.x][(int)w.y]].wall) continue;
            if (!((MOOR[i].x == 0) ||
                  (MOOR[i].y == 0) ||
                  ((map->tile[map->map[(int)(v.x + MOOR[i].x)][(int)(v.y)]].wall == 0 &&
                    map->map_green[(int)(v.x + MOOR[i].x)][(int)(v.y)] != -1) &&
                   (map->tile[map->map[(int)(v.x)][(int)(v.y + MOOR[i].y)]].wall == 0 &&
                    map->map_green[(int)(v.x)][(int)(v.y + MOOR[i].y)] != -1)))) continue;

            //kolizje z NPC
            for (int i = 0; i < game->NPC.size(); i++)
            {
                if (w.x * TILE_SIZE > game->NPC[i]->pos->x && w.x * TILE_SIZE < game->NPC[i]->pos->x && w.y * TILE_SIZE > game->NPC[i]->pos->y && w.y * TILE_SIZE < game->NPC[i]->pos->y) continue;
            }

            //kolizje zmoobami
            for (int i = 0; i < game->moobs.size(); i++)
            {
                if (disBetweenPoint(w.x * TILE_SIZE , w.y * TILE_SIZE , game->moobs[i]->pos->x, game->moobs[i]->pos->y) < TILE_SIZE) continue;//disBetweenPoint(p->x, p->y, game->moobs[i]->pos->x, game->moobs[i]->pos->y) < TILE_SIZE) return true;
            }

            tile[(int)w.x][(int)w.y] = tile[(int)v.x][(int)v.y] + 1;
            Q.push(w);

            if (w == e) break;
        }

        if (w == e && tile[(int)e.x][(int)e.y] == tile[(int)v.x][(int)v.y] + 1) break;
    }

    if (tile[(int)e.x][(int)e.y] == -1) tile[(int)e.x][(int)e.y] = tile[(int)v.x][(int)v.y] + 1;

    /*std::fstream file;
    file.open("way.txt", std::ios::out);
    for (int i = 0; i < 20; i++)
    {
        for (int j = 0; j < 20; j++)
            file << tile[j][i] << " ";
        file << "\n";
    }
    file.close();*/

    //ukladanie sciezki
    std::vector <sPoint> ret;
    if (w != e) return ret;
    v = e;

    while (true)
    {
        ret.push_back(v);
        if (v == s) break;

        for (int i = 0; i < 8; i++)
        {
            w = v + MOOR[i];
            if (!map->isOnMap(w)) continue;

            if (tile[(int)w.x][(int)w.y] == tile[(int)v.x][(int)v.y] - 1)
            {
                v = w;
                break;
            }
        }

        if (v == ret[ret.size()-1]) break;
    }

    for (int i = 0; i < ret.size(); i++)
    {
        ret[i] = ret[i] * TILE_SIZE + TILE_SIZE/2;
    }

    //spradzenie ostatniego punktu
    if (abs(disBetweenPoint(ret[0].x, ret[0].y, ret[1].x, ret[1].y)) > abs(disBetweenPoint(end.x, end.y, ret[1].x, ret[1].y)))
    {
        if (!checkCollision(map, &end))
            ret[0] = end;
    }
    else
    {
        if (!checkCollision(map, &end))
            ret.insert(ret.begin(), end);
    }

    //sprawdzenie pierwszego punktu
    if (ret.size() > 2)
        if (abs(disBetweenPoint(ret[ret.size()-1].x, ret[ret.size()-1].y, ret[ret.size()-2].x, ret[ret.size()-2].y)) >
            abs(disBetweenPoint(start.x,             start.y,             ret[ret.size()-2].x, ret[ret.size()-2].y)))
            ret.erase(ret.end()-1);

    optimizeWay(ret, map);

    return ret;
}

void cFindWay::prepareTile(sPoint * size)
{
    tile = new int*[(int)size->x];

    for (int i = 0; i < (int)size->x; i++)
    {
        tile[i] = new int[(int)size->y];

        for (int j = 0; j < (int)size->y; j++)
            tile[i][j] = UNVISITED_FIELD;
    }
}

bool cFindWay::checkCollision(cMap * map, sPoint * p)
{
    int x1, x2, y1, y2;

    x1 = p->x - TILE_SIZE / 2;
    x2 = p->x + TILE_SIZE / 2 - 1;
    y1 = p->y - TILE_SIZE / 2;
    y2 = p->y + TILE_SIZE / 2 - 1;

    if (map->isOnMap(sPoint(x1, y1))) if (map->tile[map->map[x1/TILE_SIZE][y1/TILE_SIZE]].wall || map->map_green[x1/TILE_SIZE][y1/TILE_SIZE] != -1) return true;
    if (map->isOnMap(sPoint(x1, y2))) if ((int)p->y % TILE_SIZE != 0) if (map->tile[map->map[x1/TILE_SIZE][y2/TILE_SIZE]].wall || map->map_green[x1/TILE_SIZE][y2/TILE_SIZE] != -1) return true;
    if (map->isOnMap(sPoint(x2, y1))) if ((int)p->x % TILE_SIZE != 0) if (map->tile[map->map[x2/TILE_SIZE][y1/TILE_SIZE]].wall || map->map_green[x2/TILE_SIZE][y1/TILE_SIZE] != -1) return true;
    if (map->isOnMap(sPoint(x2, y2))) if ((int)p->x % TILE_SIZE != 0 && (int)p->y % TILE_SIZE != 0) if (map->tile[map->map[x2/TILE_SIZE][y2/TILE_SIZE]].wall || map->map_green[x2/TILE_SIZE][y2/TILE_SIZE] != -1) return true;

    //kolizje z NPC
    for (int i = 0; i < game->NPC.size(); i++)
    {
        if (p->x > game->NPC[i]->pos->x - TILE_SIZE/2 && p->x < game->NPC[i]->pos->x + TILE_SIZE/2 && p->y > game->NPC[i]->pos->y - TILE_SIZE/2 && p->y < game->NPC[i]->pos->y + TILE_SIZE/2) return true;
    }

    //kolizje zmoobami
    for (int i = 0; i < game->moobs.size(); i++)
    {
        if (p->x > game->moobs[i]->pos->x - TILE_SIZE/2 && p->x < game->moobs[i]->pos->x + TILE_SIZE/2 && p->y > game->moobs[i]->pos->y - TILE_SIZE/2 && p->y < game->moobs[i]->pos->y + TILE_SIZE/2) return true;//disBetweenPoint(p->x, p->y, game->moobs[i]->pos->x, game->moobs[i]->pos->y) < TILE_SIZE) return true;//
    }

    return false;
}

bool cFindWay::obstacleBetweenPoints(sPoint a, sPoint b, cMap * map)//b -> cel
{
    float distance = disBetweenPoint(a.x, a.y, b.x, b.y);
    float angle = -countAngle(b.x-a.x, b.y-a.y);

    float step = 2.0f;

    sPoint * p = new sPoint(a.x, a.y);

    while (distance > 0)
    {
        if (checkCollision(map, p)) return true;

        p->x += cos(angle) * step;
        p->y += sin(angle) * step;

        distance -= step;
    }

    return false;
}

void cFindWay::optimizeWay(std::vector <sPoint> &way, cMap * map)
{
    for (int i = 0; i < way.size() - 1; i++)
    {
        if (i+2 >= way.size()) return;
        if (!obstacleBetweenPoints(way[i], way[i+2], map))
        {
            way.erase(way.begin() + i + 1);
            optimizeWay(way, map);
            return;
        }
    }
}
